#include "nmea0183.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

char *__NMEA0183_findNextField(char **ptr){
    *ptr = strstr((const char*)*ptr,",");
    **ptr = '\0';
    *ptr += 1;
    return *ptr;
}

static unsigned char __nmea0183_address_enbits[8];

int NMEA0183_getAddressEnable(NMEA0183Address_t address){
    int flag_bit = (int)address;
    int byte_idx = flag_bit / (sizeof(unsigned char) * 8);
    int bit_idx = flag_bit % (sizeof(unsigned char) * 8);
    if ( __nmea0183_address_enbits[byte_idx] & (0x0001 << bit_idx) ){
        return 1;
    }else{
        return 0;
    }
}

void NMEA0183_setAddressEnable(NMEA0183Address_t address,int enable){
    int flag_bit = (int)address;
    int byte_idx = flag_bit / (sizeof(unsigned char) * 8);
    int bit_idx = flag_bit % (sizeof(unsigned char) * 8);
    if ( enable ){
        __nmea0183_address_enbits[byte_idx] |= (0x0001 << bit_idx);
    }else{
        __nmea0183_address_enbits[byte_idx] &= ~(0x0001 << bit_idx);
    }
}

void NMEA0183_RMCHandler(ShipInformation_t *ship,char *message){
    char *utc_of_position_fix;
    char *status;
    char *latitude;
    char *latitude_type;
    char *longitude;
    char *longitude_type;
    char *speed_over_ground;
    char *course_over_ground;
    char *date;
    char *magnetic_variation;
    char *mode_indicator;
    double f_latitude;
    double f_longitude;
    double f_sog;
    double f_cog;
    utc_of_position_fix = message;
    status = __NMEA0183_findNextField(&message);
    latitude = __NMEA0183_findNextField(&message);
    latitude_type = __NMEA0183_findNextField(&message);
    longitude = __NMEA0183_findNextField(&message);
    longitude_type = __NMEA0183_findNextField(&message);
    speed_over_ground = __NMEA0183_findNextField(&message);
    course_over_ground = __NMEA0183_findNextField(&message);
    date = __NMEA0183_findNextField(&message);
    magnetic_variation = __NMEA0183_findNextField(&message);
    mode_indicator = __NMEA0183_findNextField(&message);
    if ( strcmp(status,"A") == 0 ){
        /* Valid data. */
        f_latitude = atof(latitude + 2) / 60;
        *(latitude + 2) = '\0';
        f_latitude += (double)atoi(latitude);
        if ( strcmp(latitude_type,"S") == 0 ){
            f_latitude = -f_latitude;
        }
        f_longitude = atof(longitude + 3) / 60;
        *(longitude + 3) = '\0';
        f_longitude += (double)atoi(longitude);
        if ( strcmp(longitude_type,"W") == 0 ){
            f_longitude = -f_longitude;
        }
        /* Update latitude and longitude. */
        ship->gnss_information.latitude = f_latitude;
        ship->gnss_information.longitude = f_longitude;
        ship->gnss_information.validity = NMEA0183_Valid;

        /* Handle sog and cog. */
        f_sog = atof(speed_over_ground);
        f_cog = atof(course_over_ground);
        ship->navigation_information.sog = f_sog;
        ship->navigation_information.cog = f_cog;
    }else{
        ship->gnss_information.validity = NMEA0183_Invalid;
    }
}

void NMEA0183_GLLHandler(ShipInformation_t *ship,char *message){
    char *latitude;
    char *latitude_type;
    char *longitude;
    char *longitude_type;
    char *utc_time;
    char *valid;
    char *mode;
    double f_latitude;
    double f_longitude;
    latitude = message;
    latitude_type = __NMEA0183_findNextField(&message);
    longitude = __NMEA0183_findNextField(&message);
    longitude_type = __NMEA0183_findNextField(&message);
    utc_time = __NMEA0183_findNextField(&message);
    valid = __NMEA0183_findNextField(&message);
    mode = __NMEA0183_findNextField(&message);
    if ( strcmp(valid,"A") == 0 ){
        /* Valid data. */
        f_latitude = atof(latitude + 2) / 60;
        *(latitude + 2) = '\0';
        f_latitude += (double)atoi(latitude);
        if ( strcmp(latitude_type,"S") == 0 ){
            f_latitude = -f_latitude;
        }
        f_longitude = atof(longitude + 3) / 60;
        *(longitude + 3) = '\0';
        f_longitude += (double)atoi(longitude);
        if ( strcmp(longitude_type,"W") == 0 ){
            f_longitude = -f_longitude;
        }
        /* Update latitude and longitude. */
        ship->gnss_information.latitude = f_latitude;
        ship->gnss_information.longitude = f_longitude;
        ship->gnss_information.validity = NMEA0183_Valid;
    }else{
        ship->gnss_information.validity = NMEA0183_Invalid;
    }
}

void NMEA0183_VTGHandler(ShipInformation_t *ship,char *message){
    char *cog_true;
    char *flag_cog_t;
    char *cog_magnetic;
    char *flag_cog_m;
    char *sog_knots;
    char *flag_sog_kn;
    char *sog_kph;
    char *flag_sog_kph;
    char *mode;
    double f_cog;
    double f_sog;
    double f_sog_kph;
    cog_true = message;
    flag_cog_t = __NMEA0183_findNextField(&message);
    cog_magnetic = __NMEA0183_findNextField(&message);
    flag_cog_m = __NMEA0183_findNextField(&message);
    sog_knots = __NMEA0183_findNextField(&message);
    flag_sog_kn = __NMEA0183_findNextField(&message);
    sog_kph = __NMEA0183_findNextField(&message);
    flag_sog_kph = __NMEA0183_findNextField(&message);
    mode = __NMEA0183_findNextField(&message);
    f_cog = atof(cog_true);
    f_sog = atof(sog_knots);
    f_sog_kph = atof(sog_kph);
    /* Update COG and SOG. */
    ship->navigation_information.cog = f_cog;
    ship->navigation_information.sog = f_sog;
    ship->navigation_information.sog_kph = f_sog_kph;
}

void NMEA0183_init(ShipInformation_t *ship){
    for ( int n = 0; n < 8; n++ ){
        __nmea0183_address_enbits[n] = 0xff;
    }
    /* Init gnss information. */
    ship->gnss_information.latitude = 0;
    ship->gnss_information.longitude = 0;
    ship->gnss_information.altitude = 0;
    ship->gnss_information.validity = NMEA0183_Invalid;
    /* Init navigation information. */
    ship->navigation_information.cog = 0;
    ship->navigation_information.sog = 0;
    ship->navigation_information.validity = NMEA0183_Invalid;
}

void NMEA0183_messageHandler(ShipInformation_t *ship,char *address,char *message){
    char *address_type = address + 2;
    printf("[NMEA0183]%.3s(%.2s)->%s\r\n",address_type,address,message);
    if ( strcmp(address_type,"GGA") == 0 && NMEA0183_getAddressEnable(NMEA_GGA) ){
        /* GGA message. */
    }else if ( strcmp(address_type,"RMC") == 0 && NMEA0183_getAddressEnable(NMEA_RMC) ){
        /* RMC message. */
        NMEA0183_RMCHandler(ship,message);
    }else if ( strcmp(address_type,"GLL") == 0 && NMEA0183_getAddressEnable(NMEA_GLL) ){
        /* GLL message. */ 
        NMEA0183_GLLHandler(ship,message);
    }else if ( strcmp(address_type,"ZDA") == 0 && NMEA0183_getAddressEnable(NMEA_ZDA) ){
        /* ZDA message. */   
    }else if ( strcmp(address_type,"WPL") == 0 && NMEA0183_getAddressEnable(NMEA_WPL) ){
        /* WPL message. */   
    }else if ( strcmp(address_type,"BWC") == 0 && NMEA0183_getAddressEnable(NMEA_BWC) ){
        /* BWC message. */   
    }else if ( strcmp(address_type,"BWR") == 0 && NMEA0183_getAddressEnable(NMEA_BWR) ){
        /* BWR message. */   
    }else if ( strcmp(address_type,"GNS") == 0 && NMEA0183_getAddressEnable(NMEA_GNS) ){
        /* GNS message. */   
    }else if ( strcmp(address_type,"RMB") == 0 && NMEA0183_getAddressEnable(NMEA_RMB) ){
        /* RMB message. */   
    }else if ( strcmp(address_type,"VTG") == 0 && NMEA0183_getAddressEnable(NMEA_VTG) ){
        /* VTG message. */   
        NMEA0183_VTGHandler(ship,message);
    }else if ( strcmp(address_type,"HDT") == 0 && NMEA0183_getAddressEnable(NMEA_HDT) ){
        /* HDT message. */   
    }else if ( strcmp(address_type,"HDM") == 0 && NMEA0183_getAddressEnable(NMEA_HDM) ){
        /* HDM message. */   
    }else if ( strcmp(address_type,"ROT") == 0 && NMEA0183_getAddressEnable(NMEA_ROT) ){
        /* ROT message. */   
    }else if ( strcmp(address_type,"HDG") == 0 && NMEA0183_getAddressEnable(NMEA_HDG) ){
        /* HDG message. */   
    }else if ( strcmp(address_type,"HTC") == 0 && NMEA0183_getAddressEnable(NMEA_HTC) ){
        /* HTC message. */   
    }else if ( strcmp(address_type,"HTD") == 0 && NMEA0183_getAddressEnable(NMEA_HTD) ){
        /* HTD message. */   
    }else if ( strcmp(address_type,"STW") == 0 && NMEA0183_getAddressEnable(NMEA_STW) ){
        /* STW message. */   
    }else if ( strcmp(address_type,"SOG") == 0 && NMEA0183_getAddressEnable(NMEA_SOG) ){
        /* SOG message. */   
    }else if ( strcmp(address_type,"VBW") == 0 && NMEA0183_getAddressEnable(NMEA_VBW) ){
        /* VBW message. */   
    }else if ( strcmp(address_type,"VHW") == 0 && NMEA0183_getAddressEnable(NMEA_VHW) ){
        /* VHW message. */   
    }else if ( strcmp(address_type,"VLW") == 0 && NMEA0183_getAddressEnable(NMEA_VLW) ){
        /* VLW message. */   
    }else if ( strcmp(address_type,"MWD") == 0 && NMEA0183_getAddressEnable(NMEA_MWD) ){
        /* MWD message. */   
    }else if ( strcmp(address_type,"MWV") == 0 && NMEA0183_getAddressEnable(NMEA_MWV) ){
        /* MWV message. */   
    }else if ( strcmp(address_type,"VWR") == 0 && NMEA0183_getAddressEnable(NMEA_VWR) ){
        /* VWR message. */   
    }else if ( strcmp(address_type,"VWT") == 0 && NMEA0183_getAddressEnable(NMEA_VWT) ){
        /* VWT message. */   
    }else if ( strcmp(address_type,"DPT") == 0 && NMEA0183_getAddressEnable(NMEA_DPT) ){
        /* DPT message. */   
    }else if ( strcmp(address_type,"DBT") == 0 && NMEA0183_getAddressEnable(NMEA_DBT) ){
        /* DBT message. */   
    }else if ( strcmp(address_type,"DBS") == 0 && NMEA0183_getAddressEnable(NMEA_DBS) ){
        /* DBS message. */   
    }else if ( strcmp(address_type,"DBK") == 0 && NMEA0183_getAddressEnable(NMEA_DBK) ){
        /* DBK message. */   
    }else if ( strcmp(address_type,"RPM") == 0 && NMEA0183_getAddressEnable(NMEA_RPM) ){
        /* RPM message. */   
    }else if ( strcmp(address_type,"XDR") == 0 && NMEA0183_getAddressEnable(NMEA_XDR) ){
        /* XDR message. */   
    }else if ( strcmp(address_type,"MDA") == 0 && NMEA0183_getAddressEnable(NMEA_MDA) ){
        /* MDA message. */   
    }

}

void NMEA0183_parseLine(ShipInformation_t *ship,char *message_line){
    char *l_header = message_line;
    char *message;
    char *checksum;
    for ( ; *l_header != '\0'; l_header++ ){
        if ( *l_header == '$' ){
            break;
        }
    }
    if ( *l_header == '\0' ){
        /* It is not a valid NMEA0183 message. */
        return;
    }
    /* Split string with *. */
    message = l_header;
    l_header = strstr(l_header,"*");
    *l_header = '\0';
    checksum = l_header + 1;
    /* Check checksum. */
    char *strnext;
    char f_checksum = 0x00;
    char r_checksum = strtol(checksum,&strnext,16);
    for ( int n = 1; message[n] != '\0'; n++ ){
        f_checksum ^= message[n];
    }
    if ( f_checksum != r_checksum ){
        /* Check checksum failed. */
        return;
    }
    message += 1;   /* Jump over first character '$'. */
    char *address = message;
    message = strstr(message,",");
    *message = '\0';
    message += 1;
    NMEA0183_messageHandler(ship,address,message);
}

void NMEA0183_parse(ShipInformation_t *ship,char *message){
    char *msg_ptr = message;
    char *msg_header = message;
    for ( ; *msg_ptr != '\0'; msg_ptr++ ){
        if ( *msg_ptr == '\n' ){
            /* Handle single line. */
            *msg_ptr = '\0';
            NMEA0183_parseLine(ship,msg_header);
            msg_header = msg_ptr + 1;        
        }
    }
    if ( *msg_header != '\0' ){
        NMEA0183_parseLine(ship,msg_header);
    }
    return;
} 
